#ifndef __LOCAL_PLANNER_BASE_H__
#define __LOCAL_PLANNER_BASE_H__

#include <ros/ros.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h> 
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/LaserScan.h>

#include <iostream>
#include <vector>
using namespace std;
#include "../../core/core.h"

// 局部路径规划类型
enum class LocalPlannerType{
    DWA = 0,
};


class LocalPlannerBase{
    public:
        LocalPlannerBase(tf2_ros::Buffer* tf_buffer):__tf_buffer(tf_buffer){};
        virtual ~LocalPlannerBase() = default;    

        // 更新缓存数据
        virtual void UpdateCache() = 0;
        // 计算速度指令
        virtual bool ComputeTwist(Velocity2D& cmd_vel, LidarScan2D lidar_scan_2d, 
            Pose2D robot_pose, Pose2D goal_pose) = 0; 

        // 获取机器人当前位姿
        bool GetRobotPose(geometry_msgs::PoseStamped& global_pose); 
        // 获取TF坐标变换
        bool GetTFPose(geometry_msgs::PoseStamped& global_pose, string father_frame, string child_frame,
            ros::Time stamp = ros::Time(0)); 

        // 机器人停止
        bool Stop(Velocity2D& cmd_vel, bool e_stop, Velocity2D curr_vel, Pose2D curr, Pose2D goal);
        // 旋转到目标点位姿 的周期速度
        bool RotateToGoal(Velocity2D& cmd_vel, Pose2D curr, Pose2D goal);

        // 符号判断
        inline float sign(float x){
            return x < 0.0 ? -1.0 : 1.0;
        }
        // 设置地图数据
        void SetMap2D(Map2D map_2d){
            this->map_2d = map_2d;
            UpdateCache();  //更新缓存数据
        }

        Map2D map_2d;                           //二维栅格地图数据

    private:
        tf2_ros::Buffer* const __tf_buffer;

    protected:
        Velocity2D __max_vel_limit = Velocity2D(0.8, 0.8, 1.2);       //最大速度限制
        Velocity2D __min_vel_limit = Velocity2D(-0.8, -0.8, -1.2);    //最小速度限制
        float __transform_tolerance = 0.2;
        string __base_frame = "base_link";      //局部坐标系 一般为base_link
        string __map_frame = "map";             //全局坐标系 一般为map
        float __controller_frequency = 0.05;   //20Hz
};


#endif